#pragma once
#include <common.hpp>
#include <Math/Vector3.hpp>
#include <Math/Matrix3x3.hpp>
#include "../VisionTools.hpp"

namespace zzz{
class Homography
{
public:
  static const int MIN_POINT_NUMBER = 4;

  Homography(){Hab_.Identical(); Hba_.Identical();}
  bool Create(const vector<Vector2d> &pa, const vector<Vector2d> &pb);
  bool Create(const vector<Vector3d> &pa, const vector<Vector3d> &pb);
  Vector3d ToA(const Vector3d &pb) {
    Vector3d ret=Hba_*pb;
    ret/=ret[2];
    return ret;
  }
  Vector2d ToA(const Vector2d &pb) {
    return FromHomogeneous(ToA(ToHomogeneous(pb)));
  }
  Vector3d ToB(const Vector3d &pa) {
    Vector3d ret=Hab_*pa;
    ret/=ret[2];
    return ret;
  }
  Vector2d ToB(const Vector2d &pb) {
    return FromHomogeneous(ToB(ToHomogeneous(pb)));
  }
  Matrix3x3d GetHab(){return Hab_;}
  Matrix3x3d GetHba(){return Hba_;}
private:
  Matrix3x3d Hab_;
  Matrix3x3d Hba_;
};
}